#include "precompiled.h"
#pragma hdrstop

namespace inf {
    const Quat Quat::ORIGIN(0.0f,0.0f,0.0f,0.0f);
    const Quat Quat::ONE(1.0f, 1.0f, 1.0f, 1.0f);

    /*
    =====================
    Quat::to_Angles
    =====================
    */
    Angles Quat::to_Angles( void ) const {
        return to_Mat3().to_Angles();
    }

    /*
    =====================
    Quat::to_Rotation
    =====================
    */
    Rotation Quat::to_Rotation( void ) const {
        Vec3 vec;
        float angle;

        vec.x = x;
        vec.y = y;
        vec.z = z;
        angle = math::acos( w );
        if ( angle == 0.0f ) {
            vec.set( 0.0f, 0.0f, 1.0f );
        } else {
            //vec *= (1.0f / sin( angle ));
            vec.normalize();
            vec.fix_degenerate_normal();
            angle *= 2.0f * math::M_RAD2DEG;
        }
        return Rotation( Vec3::ORIGIN, vec, angle );
    }

    /*
    =====================
    Quat::to_Mat3
    =====================
    */
    Mat3 Quat::to_Mat3( void ) const {
        Mat3	mat;
        float	wx, wy, wz;
        float	xx, yy, yz;
        float	xy, xz, zz;
        float	x2, y2, z2;

        x2 = x + x;
        y2 = y + y;
        z2 = z + z;

        xx = x * x2;
        xy = x * y2;
        xz = x * z2;

        yy = y * y2;
        yz = y * z2;
        zz = z * z2;

        wx = w * x2;
        wy = w * y2;
        wz = w * z2;

        mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
        mat[ 0 ][ 1 ] = xy - wz;
        mat[ 0 ][ 2 ] = xz + wy;

        mat[ 1 ][ 0 ] = xy + wz;
        mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
        mat[ 1 ][ 2 ] = yz - wx;

        mat[ 2 ][ 0 ] = xz - wy;
        mat[ 2 ][ 1 ] = yz + wx;
        mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );

        return mat;
    }

    /*
    =====================
    Quat::to_Mat4
    =====================
    */
    Mat4 Quat::to_Mat4( void ) const {
        return to_Mat3().to_Mat4();
    }

    /*
    =====================
    Quat::to_CQuat
    =====================
    */
    CQuat Quat::to_CQuat( void ) const {
        if ( w < 0.0f ) {
            return CQuat( -x, -y, -z );
        }
        return CQuat( x, y, z );
    }

    /*
    ============
    Quat::to_angular_velocity
    ============
    */
    Vec3 Quat::to_angular_velocity( void ) const {
        Vec3 vec;
        float angle;

        //
        // FeaRog:
        //		Negative w basically indicates reverse rotation
        //		acos will return a value in the range PI/2 <= x <= PI for negative w
        //		If this function doubled the angle (which is mathematically correct -> theta = 2*acos(w) )
        //		and then normalized it so it was in the range -PI/2 <= x <= PI/2
        //		then this function would return the correct value.
        //
        //		However its easier & faster just to do the one branch and invert the axis
        //		
        if ( w > 0.0f ) {
            vec.x = x;
            vec.y = y;
            vec.z = z;
            angle = math::acos( w );
        } else {
            vec.x = -x;
            vec.y = -y;
            vec.z = -z;
            angle = math::acos( -w );
        }

        vec.normalize();
        return vec * angle;
    }

    /*
    =============
    Quat::to_string
    =============
    */
    const char *Quat::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }

    /*
    =============
    CQuat::to_Angles
    =============
    */
    Angles CQuat::to_Angles( void ) const {
        return to_Quat().to_Angles();
    }

    /*
    =============
    CQuat::to_Rotation
    =============
    */
    Rotation CQuat::to_Rotation( void ) const {
        return to_Quat().to_Rotation();
    }

    /*
    =============
    CQuat::to_Mat3
    =============
    */
    Mat3 CQuat::to_Mat3( void ) const {
        return to_Quat().to_Mat3();
    }

    /*
    =============
    CQuat::to_Mat4
    =============
    */
    Mat4 CQuat::to_Mat4( void ) const {
        return to_Quat().to_Mat4();
    }

    /*
    =============
    CQuat::to_string
    =============
    */
    const char *CQuat::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }

    /*
    =====================
    Quat::slerp

    Spherical linear interpolation between two quaternions.
    =====================
    */
    Quat &Quat::slerp( const Quat &from, const Quat &to, float t ) {
        float cosom, absCosom, sinom, omega, scale0, scale1;

        if ( t <= 0.0f ) {
            *this = from;
            return *this;
        }

        if ( t >= 1.0f ) {
            *this = to;
            return *this;
        }

        if ( from == to ) {
            *this = to;
            return *this;
        }

        cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
        absCosom = math::fabs( cosom );

        if ( ( 1.0f - absCosom ) > 1e-6f ) {
#if 0
            omega = acos( absCosom );
            sinom = 1.0f / sin( omega );
            scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
            scale1 = sin( t * omega ) * sinom;
#else
            scale0 = 1.0f - absCosom * absCosom;
            sinom = math::inv_sqrt( scale0 );
            omega = math::atan16( scale0 * sinom, absCosom );
            scale0 = math::sin16( ( 1.0f - t ) * omega ) * sinom;
            scale1 = math::sin16( t * omega ) * sinom;
#endif
        } else {
            scale0 = 1.0f - t;
            scale1 = t;
        }

        scale1 = ( cosom >= 0.0f ) ? scale1 : -scale1;

        x = scale0 * from.x + scale1 * to.x;
        y = scale0 * from.y + scale1 * to.y;
        z = scale0 * from.z + scale1 * to.z;
        w = scale0 * from.w + scale1 * to.w;

        return *this;
    }

    /*
    =====================
    Quat::slerp_fast

    Approximation of spherical linear interpolation between two quaternions.
    The interpolation traces out the exact same curve as slerp but does not maintain a constant speed across the arc.
    =====================
    */
    Quat &Quat::slerp_fast( const Quat &from, const Quat &to, float t ) {
        float cosom, scale0, scale1, s;

        if ( t <= 0.0f ) {
            *this = from;
            return *this;
        }

        if ( t >= 1.0f ) {
            *this = to;
            return *this;
        }

        if ( from == to ) {
            *this = to;
            return *this;
        }

        cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;

        scale0 = 1.0f - t;
        scale1 = ( cosom >= 0.0f ) ? t : -t;

        x = scale0 * from.x + scale1 * to.x;
        y = scale0 * from.y + scale1 * to.y;
        z = scale0 * from.z + scale1 * to.z;
        w = scale0 * from.w + scale1 * to.w;

        s = math::inv_sqrt( x * x + y * y + z * z + w * w );

        x *= s;
        y *= s;
        z *= s;
        w *= s;

        return *this;
    }

}   // ::inf
